#include "Vision.h"


const ParticleFilterCriteria2 Vision::FilterCriteria2[] =  
{
	{IMAQ_MT_AREA,0,200,false,false}
};
const static ParticleFilterOptions2 PFO =
{
		true,
		true,
		false,
		true
};
const ParticleFilterOptions2 *Vision::FilterOptions2 = &PFO; 



Vision::Vision()
{
	//HSLMask = new Threshold(136,182,45,255,116,255);
	HSLMask = new Threshold(98, 139, 59, 255, 50, 255);
	FilterCriteria.parameter = IMAQ_IMAGE_AREA;
	FilterCriteria.lower = 0;
	FilterCriteria.upper = 200;
	
	criteriaCount = 1;
	
	
	filterCount = 1;
	
	
	
	
	//Tested values are: hue: min98,max119 ; sat: min59,max255 ; lum: min50,max255
	//Other  values are 98, 139, 59, 255, 50, 255
	//these values are tested for greatest red conc in the target zone... note some ambient light causes red pixelation
	//Test 2 indicates that multiple hole fillers in the Adv Morphological state yields greater filling and accuracy of area
}
point_t Vision::TopHoop(HSLImage* img)
{
	return SolveEverything(img);
}
point_t Vision::SolveEverything(HSLImage* img)
{
	/*
	 * Get image (passed into me)
	 * Mask in HSL to elim anything other than bright reflective tape
	 * If clean enough send to convex hull
	 * extract plane with vector analysis
	 * calculate theta between camera dead-ahead and normal to goal plane
	 * do 4 targets exist? if so you have all the targets
	 * If less send rectangle params to math solver to "create virtual targets"
	 *
	 */
	{	
		printf("solver called\n");
		point_t tmp = {0,0};
		BinaryImage* mask;
		img->Write("base.png");
		mask = img->ThresholdHSL(*HSLMask);
		mask->Write("image1.png"); // subsequent code tranposes the image to imaq format so national instruments can process it
		imaqConvexHull(mask->GetImaqImage(), mask->GetImaqImage(), 8);
		mask->Write("image2.png");
		
		printf("Solver Checkpoint 1\n");	
		imaqMeasureParticles(mask->GetImaqImage(), calibrationMode, measurements, numMeasurements);
		//delete result;
		printf("Solver Checkpoint 1.25\n");
		numMeasurements = mask->GetNumberParticles();
		printf("particles before filter:%d\n",numMeasurements);
		//FILTER ... RIO = region of interest
		imaqParticleFilter4(mask->GetImaqImage(), mask->GetImaqImage(), FilterCriteria2 , criteriaCount, FilterOptions2, NULL, (int*)numMeasurements);
		mask->Write("image3.png");
		printf("Solver Checkpoint 1.5\n");
		imaqConvexHull(mask->GetImaqImage(), mask->GetImaqImage(), 8); // 2 pixels thta are surrounding pixels ... think tic-tac-toe
		mask->Write("image4.png");
		//MeasureParticlesReport* finalAnalysisReport = imaqMeasureParticles(mask->GetImaqImage(), calibrationMode, measurements, numMeasurements);
		
		numMeasurements = mask->GetNumberParticles();
		printf("particles after filter:%d\n",numMeasurements);
		ParticleAnalysisReport finalAnalysisReport[numMeasurements];
		UINT32 i = 0;
		printf("Solver Checkpoint 1.75\n");
		if (numMeasurements == 0)
		{
			return tmp;
		}
		for (i = 0; i < numMeasurements-1; i++)
		{
			printf("%d ",i);
			finalAnalysisReport[i] = mask->GetParticleAnalysisReport(i);
		}
		printf("\nSolver Checkpoint 2\n");
		tmp.x = finalAnalysisReport[0].center_mass_x;
		tmp.y = finalAnalysisReport[0].center_mass_y;
		free(finalAnalysisReport);
		delete img;
		delete mask;
		return tmp;
	}
};
